#include <QCoreApplication>
#include "sixjointrobot.h"
#include <QDebug>
#include <kdl/frames_io.hpp>
#include "measuretime.h"
//#include </tcrwork/linuxController/trunk/TcrController/include/kdl/jntarray.hpp>

using namespace KDL;

SixJointRobot sixJointRobot;

int getIkPos_SIX(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{

//    SixJointRobot sixJointRobot;

//    //初始化ＤＨ参数
//    //float tmpDhParameter[6][4];
//    float tmpDhParameter[6][4] = {0.175, 1.570796327, 0.3355, 0,
//                                          1.095, 0, 0, 1.570796327,
//                                          0.175, 1.570796327, 0, 0,
//                                          0, -1.570796327, 0.9299, 0,
//                                          0, 1.570796327, 0, 0,
//                                          0, 0, 0, 0};
//    sixJointRobot.initialForIk(tmpDhParameter);

    //初始化中间变量
    float tmpPoseMatrix[4][4];
    float tmpBeforeJoint[6];
    float tmpJointSolution[6];

    tmpPoseMatrix[0][0]=p_in.M.data[0];
    tmpPoseMatrix[0][1]=p_in.M.data[1];
    tmpPoseMatrix[0][2]=p_in.M.data[2];
    tmpPoseMatrix[1][0]=p_in.M.data[3];
    tmpPoseMatrix[1][1]=p_in.M.data[4];
    tmpPoseMatrix[1][2]=p_in.M.data[5];
    tmpPoseMatrix[2][0]=p_in.M.data[6];
    tmpPoseMatrix[2][1]=p_in.M.data[7];
    tmpPoseMatrix[2][2]=p_in.M.data[8];

    tmpPoseMatrix[0][3]=p_in.p[0];
    tmpPoseMatrix[1][3]=p_in.p[1];
    tmpPoseMatrix[2][3]=p_in.p[2];
    tmpPoseMatrix[3][3]=1;
    tmpPoseMatrix[3][0]=0;
    tmpPoseMatrix[3][1]=0;
    tmpPoseMatrix[3][2]=0;


    for(int j=0;j<6;j++)
    {
        tmpBeforeJoint[j]=q_init(j);
    }

    //求解
    if(1!=sixJointRobot.getIkSolution(tmpPoseMatrix,tmpBeforeJoint,tmpJointSolution))
    {
        qDebug()<<"get ik failed!";
        return -1;
    }

    //求解值转换
    for(int i=0;i<6;i++)
    {
            joints(i)=tmpJointSolution[i];
    }

    return 0;

}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);
    MeasureTime* measureTime = new MeasureTime("testSixIkFk", 1, 1, 1e4);


    //目标位置关节值
    float jointValue[6] = {25.984/180*M_PI, 1.173/180*M_PI, -0.107/180*M_PI, 50.0/180*M_PI, -12.376/180*M_PI, -40.0/180*M_PI};
    //float jointValue[6] = {90.0/180*M_PI, -50.5/180*M_PI, 1.3847797, -70.2/180*M_PI, 150.3/180*M_PI, -88.5/180*M_PI};
    for (int i =0; i <6; i++)
    {
        qDebug() << jointValue[i];
    }
    float poseMatrix[4][4];

    //初始化ＤＨ参数
    //float tmpDhParameter[6][4];
//    float tmpDhParameter[6][4] = {0.175, 1.570796327, 0.3355, 0,
//                                          1.095, 0, 0, 1.570796327,
//                                          0.175, 1.570796327, 0, 0,
//                                          0, -1.570796327, 0.9299, 0,
//                                          0, 1.570796327, 0, 0,
//                                          0, 0, 0, 0};
    float tmpDhParameter[6][4] = {0.075, 1.570796327, 0.335, 0,
                                                            0.365, 0, 0, 1.570796327,
                                                            0.090, 1.570796327, 0, 0,
                                                            0, -1.570796327, 0.405, 0,
                                                            0, 1.570796327, 0, 0,
                                                            0, 0, 0, 0};
    sixJointRobot.initialForIk(tmpDhParameter);

//    for (int i = 0; i < 1e5; i++)
//    {
//        measureTime->recordInTime();
//        //正解
//        sixJointRobot.calculatePosition(jointValue, poseMatrix);
//        measureTime->recordOutTime();
//    }
    sixJointRobot.calculatePosition(jointValue, poseMatrix);
    for (int i = 0; i < 3; i++)
    {
        qDebug() << "-----位置pos" << i << ":----" << poseMatrix[i][3];
     }

    JntArray beforeJoint(6);
    //起始位置关节值
    float beforeJointMatrix[6] = {25.984/180*M_PI, 1.173/180*M_PI, -0.107/180*M_PI, 50.0/180*M_PI, -12.376/180*M_PI, -40.0/180*M_PI};
    for (int i = 0; i < 6; i++)
    {
        beforeJoint(i) = beforeJointMatrix[i];
    }
    JntArray poseJoint(6);
    Rotation tmpRotation = Rotation(poseMatrix[0][0], poseMatrix[0][1], poseMatrix[0][2],
                                                            poseMatrix[1][0], poseMatrix[1][1], poseMatrix[1][2],
                                                            poseMatrix[2][0], poseMatrix[2][1], poseMatrix[2][2]);
//    Rotation tmpRotation = Rotation(poseMatrix[0][0], poseMatrix[1][0], poseMatrix[2][0],
//                                                            poseMatrix[0][1], poseMatrix[1][1], poseMatrix[2][1],
//                                                            poseMatrix[0][2], poseMatrix[1][2], poseMatrix[2][2]);
    Vector tmpVector = Vector(poseMatrix[0][3], poseMatrix[1][3], poseMatrix[2][3]);
    //Vector tmpVector = Vector(0, 0, 0.889);
    Frame poseFrame = Frame(tmpRotation, tmpVector);

    for (int i = 0; i < 3; i++)
    {
        qDebug() << "-----pos" << i << ":----" << poseFrame.p[i];
     }

    for (int i = 0; i < 1e5; i++)
    {
        measureTime->recordInTime();
        getIkPos_SIX(beforeJoint, poseFrame, poseJoint);
        measureTime->recordOutTime();
    }


    for (int i = 0; i < 6; i++)
    {
        qDebug() << "-----关节" << i << ":----" << poseJoint(i);
     }


    return a.exec();
}


